// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HorizontalDepthProfileTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.UnitTests
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The Horizontal Depth Profile unit test class
    /// </summary>
    [TestClass]
    public class HorizontalDepthProfileTest
    {
        /// <summary>
        /// Test HorizontalDepthProfile.GetFOVWidthForDepth()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpKinect_GetFOVWidthForDepth()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters();
            KinectHorizontalDepthProfile hdpKinect = new KinectHorizontalDepthProfile(new Pose2D(), intrinsicParams);
            double width = hdpKinect.GetFOVWidthForDepth(10);
            Assert.IsTrue(width > 10 && width < 12);
        }

        /// <summary>
        /// Test HorizontalDepthProfile.GetFOVWidthForDepth()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpLaser_GetFOVWidthForDepth()
        {
            LaserHorizontalDepthProfile hdpLaser = new LaserHorizontalDepthProfile(new Pose2D());
            double width = hdpLaser.GetFOVWidthForDepth(100);
            Assert.IsTrue(width == 200);
        }

        /// <summary>
        /// Test HorizontalDepthProfile.GetDepthValues()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpKinect_GetDepthValues()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters();
            ImageFrameDepth depthImage = new ImageFrameDepth(intrinsicParams.HorizontalResolution, 1);
            for (int i = 0; i < intrinsicParams.HorizontalResolution; i++)
            {
                depthImage[i] = 1000; // mm
            }

            KinectHorizontalDepthProfile hdpKinect = new KinectHorizontalDepthProfile(depthImage, Pose.Identity, 0, intrinsicParams);
            for (int i = intrinsicParams.DeadPixelCount; i < intrinsicParams.HorizontalResolution; i++)
            {
                double width = (intrinsicParams.HorizontalCenter + intrinsicParams.DeadPixelCount / 2 - i) * intrinsicParams.InverseHorizontalFocalLengthInPixels;
                double expectedValue = Math.Sqrt(width * width + 1);
                Assert.AreEqual(expectedValue, hdpKinect.GetHitDistance(i));
            }
         }

        /// <summary>
        /// Test HorizontalDepthProfile.GetDepthValues()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpLaser_GetDepthValues()
        {
            double[] depths = new double[180];
            for (int i = 0; i < 180; i++)
            {
                depths[i] = 1;
            }

            LaserHorizontalDepthProfile hdp = new LaserHorizontalDepthProfile(depths, new Pose2D());
            for (int i = 0; i < 180; i++)
            {
                Assert.AreEqual(depths[i], hdp.GetHitDistance(i));
            }
        }

        /// <summary>
        /// Test HorizontalDepthProfile.GetHitPoints() 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpKinect_GetHitPoints()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters();
            ImageFrameDepth depthImage = new ImageFrameDepth(intrinsicParams.HorizontalResolution, 1);
            for (int i = 0; i < intrinsicParams.HorizontalResolution; i++)
            {
                depthImage[i] = 1000; // mm
            }

            KinectHorizontalDepthProfile hdpKinect = new KinectHorizontalDepthProfile(depthImage, Pose.Identity, 0, intrinsicParams);
            for (int i = intrinsicParams.DeadPixelCount; i < intrinsicParams.HorizontalResolution; i++)
            {
                double width = (i - intrinsicParams.HorizontalCenter - intrinsicParams.DeadPixelCount / 2) * intrinsicParams.InverseHorizontalFocalLengthInPixels;
                Point2D point = hdpKinect.GetHitPoint(i);

                Assert.AreEqual(1, point.X, MathConstants.ErrorEpsilon);
                Assert.AreEqual(width, point.Y, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test HorizontalDepthProfile.GetHitPoints() 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpLaser_GetHitPoints()
        {
            const int Width = 180;
            const int HalfWidth = Width / 2;
            double[] depths = new double[Width];
            for (int i = 0; i < Width; i++)
            {
                depths[i] = 1;
            }

            LaserHorizontalDepthProfile hdp = new LaserHorizontalDepthProfile(depths, new Pose2D());
            for (int i = 0; i < Width; i++)
            {
                double width = Math.Sin(MathConstants.Degrees2Radians * (HalfWidth - i));
                double height = Math.Cos(MathConstants.Degrees2Radians * (HalfWidth - i));
                Point2D point = hdp.GetHitPoint(i);
                Assert.AreEqual(height, point.X);
                Assert.AreEqual(width, point.Y);
            }
        }

        /// <summary>
        /// Test HorizontalDepthProfile.AddHitPoint() by verifying that adding hit points results in the same depths 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpKinect_AddHitPoint()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters();
            ImageFrameDepth depthImage = new ImageFrameDepth(intrinsicParams.HorizontalResolution, 1);
            for (int i = 0; i < intrinsicParams.HorizontalResolution; i++)
            {
                depthImage[i] = (short)(1000 + i); // mm
            }

            KinectHorizontalDepthProfile hdpKinect = new KinectHorizontalDepthProfile(depthImage, Pose.Identity, 0, intrinsicParams);
            KinectHorizontalDepthProfile hdpKinect2 = (KinectHorizontalDepthProfile)hdpKinect.Create();
            for (int i = intrinsicParams.DeadPixelCount; i < intrinsicParams.HorizontalResolution; i++)
            {
                Point2D point = hdpKinect.GetHitPoint(i);
                hdpKinect2.AddHitPoint(point);
            }

            // we expect the depths to be the same now
            for (int i = intrinsicParams.DeadPixelCount; i < intrinsicParams.HorizontalResolution; i++)
            {
                Assert.AreEqual(hdpKinect.GetHitDistance(i), hdpKinect2.GetHitDistance(i), MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test HorizontalDepthProfile.AddHitPoint() by verifying that adding hit points results in the same depths 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpLidar_AddHitPoint()
        {
            const int Width = 180;
            double[] depths = new double[Width];
            for (int i = 0; i < Width; i++)
            {
                depths[i] = 1 + 1 / (i + 1);
            }

            LaserHorizontalDepthProfile hdp = new LaserHorizontalDepthProfile(depths, new Pose2D());
            LaserHorizontalDepthProfile hdp2 = (LaserHorizontalDepthProfile)hdp.Create();
            for (int i = 0; i < Width; i++)
            {
                Point2D point = hdp.GetHitPoint(i);
                hdp2.AddHitPoint(point);
            }

            // we expect the depths to be the same now
            for (int i = 0; i < Width; i++)
            {
                Assert.AreEqual(hdp.GetHitDistance(i), hdp2.GetHitDistance(i), MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Re-projects a point along the ray and ensures the distance is computed correctly
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpKinect_Reproject()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters();
            ImageFrameDepth depthImage = new ImageFrameDepth(intrinsicParams.HorizontalResolution, 1);
            for (int i = 0; i < intrinsicParams.HorizontalResolution; i++)
            {
                depthImage[i] = (short)(2000 + i); // mm
            }

            double expectedDistance = 1;

            KinectHorizontalDepthProfile hdpKinect = new KinectHorizontalDepthProfile(depthImage, Pose.Identity, 0, intrinsicParams);
            for (int i = intrinsicParams.DeadPixelCount; i < hdpKinect.Length; i++)
            {
                // pick a point along the ray at the expected distance from the point in the HDP
                double distance = hdpKinect.GetHitDistance(i);
                Point2D point = hdpKinect.GetHitPoint(i);
                double sin = point.Y / distance;
                double cos = point.X / distance;

                double width = expectedDistance * sin;
                double height = expectedDistance * cos;

                double actualDistance;
                hdpKinect.Reproject(new Point2D(height, width), out actualDistance);

                Assert.AreEqual(expectedDistance, actualDistance, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Re-projects a point along the ray and ensures the distance is computed correctly
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpLidar_Reproject()
        {
            const int Width = 180;
            double[] initialDepths = new double[Width];
            for (int i = 0; i < Width; i++)
            {
                initialDepths[i] = 2 + 1 / (i + 1);
            }

            double expectedDistance = 1;
            LaserHorizontalDepthProfile hdp = new LaserHorizontalDepthProfile(initialDepths, new Pose2D());
            for (int i = 0; i < Width; i++)
            {
                // pick a point along the ray at the expected distance from the point in the HDP
                double depth = hdp.GetHitDistance(i);
                Point2D point = hdp.GetHitPoint(i);
                double sin = point.Y / depth;
                double cos = point.X / depth;
                double width = expectedDistance * sin;
                double height = expectedDistance * cos;

                double actualDistance;
                hdp.Reproject(new Point2D(height, width), out actualDistance);

                Assert.AreEqual(expectedDistance, actualDistance, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test HorizontalDepthProfile.GetDistanceToPoint()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpKinect_GetDistanceToPoint()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters();
            ImageFrameDepth depthImage = new ImageFrameDepth(intrinsicParams.HorizontalResolution, 1);
            for (int i = 0; i < intrinsicParams.HorizontalResolution; i++)
            {
                depthImage[i] = (short)(2000 + i); // mm
            }

            double offset = 1; // meters

            KinectHorizontalDepthProfile hdpKinect = new KinectHorizontalDepthProfile(depthImage, Pose.Identity, 0, intrinsicParams);
            for (int i = intrinsicParams.DeadPixelCount; i < intrinsicParams.HorizontalResolution; i++)
            {
                // pick a point along the ray at the expected distance from the point in the HDP
                double depth = hdpKinect.GetHitDistance(i);
                Point2D point = hdpKinect.GetHitPoint(i);
                double sin = point.Y / depth;
                double cos = point.X / depth;
                double pointDepthOnRay = depth - offset;

                double width = pointDepthOnRay * sin;
                double height = pointDepthOnRay * cos;

                double actualDistance;
                hdpKinect.Reproject(new Point2D(height, width), out actualDistance);

                Assert.AreEqual(pointDepthOnRay, actualDistance, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test HorizontalDepthProfile.GetDistanceToPoint()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void HdpLidar_GetDistanceToPoint()
        {
            const int Width = 180;
            double[] initialDepths = new double[Width];
            for (int i = 0; i < Width; i++)
            {
                initialDepths[i] = 2 + 1 / (i + 1);
            }

            double offset = 1;
            LaserHorizontalDepthProfile hdp = new LaserHorizontalDepthProfile(initialDepths, new Pose2D());
            for (int i = 0; i < Width; i++)
            {
                // pick a point along the ray at the expected distance from the point in the HDP
                double depth = hdp.GetHitDistance(i);
                Point2D point = hdp.GetHitPoint(i);
                double sin = point.Y / depth;
                double cos = point.X / depth;
                double pointDepthOnRay = depth - offset;
                double width = pointDepthOnRay * sin;
                double height = pointDepthOnRay * cos;

                double actualDistance;
                hdp.Reproject(new Point2D(height, width), out actualDistance);

                Assert.AreEqual(pointDepthOnRay, actualDistance, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Generates a parameter file for the given depth camera intrinsic parameters
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GenerateIntrinsicParameterFile()
        {
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters(577.355, 576.301, 325.923, 251.272);
            Serializer.Save("DepthCameraIntrinsics.xml", intrinsicParams);
        }
    }
}
